/*!
  @file motion_estimator.h
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#ifndef _MOTION_ESTIMATOR_H_
#define _MOTION_ESTIMATOR_H_

#include <boost/shared_ptr.hpp>
#include <boost/scoped_ptr.hpp>

#include "mrover_vo/camera_params.h"
#include "mrover_vo/feature.h"
#include "mrover_vo/feature_matcher.h"
#include "mrover_vo/validity_checker-impl.h"

#include "mrover_vo/motion_solver_p3p.h"
#include "mrover_vo/motion_solver_ransac.h"

namespace vo {

/*! 
 *  @brief  Motion esimtaion class
 *  @author Kyohei Otsu <kyon@ac.jaxa.jp>
 *  @date   2013
 *  
 *  This class performs feature tracking and relative pose estimation
 */

class MotionEstimator {
  typedef typename Feature::Ptr FeaturePtr;
  typedef typename Feature::ConstPtr FeatureConstPtr;
  typedef std::vector<int> Indices;
  typedef boost::shared_ptr<Indices> IndicesPtr;

 public:
  typedef boost::scoped_ptr<MotionEstimator> Ptr;

  MotionEstimator() 
      : tracker_(new FeatureMatcher)
      , solver_(new MotionSolverP3P)
      //, solver_(new MotionSolverRANSAC<double>)
      , adjuster_(new MotionSolverP3P)
      , matches_(new Feature::Matches)
      , acc_pose_(cv::Mat::eye(4, 4, CV_32F)) 
      , inliers_(new Indices)
      , counter_(0)
  { 
    boost::shared_ptr<ValidityCheckerForTracking> valid_chk(new ValidityCheckerForTracking);
    valid_chk->setRadius(100);
    tracker_->addValidityCheck(valid_chk);
  }

  ~MotionEstimator() { }

  //! Computes relative pose from current to previous
  /*! pose == prevTcurr */
  inline void compute(cv::Mat &pose) {
    setOutputPose(pose);
    compute();
  }
  void compute();

  void setInputFeature(const FeatureConstPtr &feature) { 
    input_ = feature; 
    if (!key_input_) resetKeyFrame(input_);
  } 
  
  void setTargetFeature(const FeatureConstPtr &feature) { target_ = feature; }
  void setOutputPose(cv::Mat &pose) { pose_ = pose; }

  void resetKeyFrame(const FeatureConstPtr &feature) {
    key_input_ = feature;
    acc_pose_ = cv::Mat::eye(4, 4, CV_32F);
    counter_ = 0;
  }

  void setCameraParams(const CameraParams::ConstPtr &cam_params) {
    cam_params_ = cam_params;
    solver_->setCameraParams(cam_params_);
    adjuster_->setCameraParams(cam_params_);
  }

  boost::shared_ptr<Feature::Matches> getMatches() const { return matches_; }
  void getPose(cv::Mat &pose) const { pose = pose_; }

  boost::shared_ptr<Indices> getInlierIndices() {
    if (!inliers_) inliers_.reset(new Indices);
    solver_->getInlierIndices(*inliers_);
    return inliers_;
  }


 private:
  inline bool initCompute() {
    if (!input_ || !target_ || !matches_ || !cam_params_) return false;
    return true;
  }
  void trackFeature();

  FeatureMatcher::Ptr tracker_;
  MotionSolver::Ptr solver_;
  MotionSolverP3P::Ptr adjuster_;

  CameraParams::ConstPtr cam_params_;

  FeatureConstPtr input_;
  FeatureConstPtr target_;
  FeatureConstPtr key_input_;
  boost::shared_ptr<Feature::Matches> matches_;
  cv::Mat pose_;
  cv::Mat acc_pose_;
  IndicesPtr inliers_;

  int counter_;
  enum { MAX_COUNT = 4, };
};

}  // namespace vo

#endif
